
#include <iostream>
#include <string>
#include "vector4.h"
#include "matrix44.h"

Vector4::Vector4( ) {
	_v[0] = _v[1] = _v[2] = 0.f;
	_v[3] = 1.f;
} Vector4::Vector4(float x, float y, float z) {
	_v[0] = x;
	_v[1] = y;
	_v[2] = z;
	_v[3] = 1.f;
} Vector4::Vector4(float x, float y, float z, float w) {
	_v[0] = x;
	_v[1] = y;
	_v[2] = z;
	_v[3] = w;
}

void Vector4::operator=(Vector4 v)
{
	this->_v[0] = v[0];
	this->_v[1] = v[1];
	this->_v[2] = v[2];
	this->_v[3] = v[3];
}
Vector4 Vector4::operator+(Vector4 v)
{
	Vector4 add = Vector4();
	add.set_x(v[0]+_v[0]);
	add.set_y(v[1]+_v[1]);
	add.set_z(v[2]+_v[2]);
	return add;
}


// Scale vector by s
void Vector4::scale(float s)
{
	for(int i=0; i<3; i++)
		_v[i] *= s;
}


void Vector4::reset()
{
	this->_v[0] = 0.f;
	this->_v[1] = 0.f;
	this->_v[2] = 0.f;
	this->_v[3] = 0.f;
}

float Vector4::dot(Vector4 v)
{
	return (_v[0]*v[0]) + // x1*x2
		   (_v[1]*v[1]) + // y1*y2
		   (_v[2]*v[2]) + // z1*z2
		   (_v[3]*v[3]);  // w1*w2
}
float Vector4::dot(float v[4])
{
	return (_v[0]*v[0]) + // x1*x2
		   (_v[1]*v[1]) + // y1*y2
		   (_v[2]*v[2]) + // z1*z2
		   (_v[3]*v[3]);  // w1*w2
}

void Vector4::println() { 
	print();
	std::printf("\n");
}
void Vector4::print() {
	std::printf("( %.3f, %.3f, %.3f, %.3f )", _v[0], _v[1], _v[2], _v[3]);
}

void RungeKuttaIntegrate(Vector4 *position, Vector4 *velocity, Vector4 *acceleration, float dt)
{
	for(int i = 0; i < 3; i++){
		//k1 component
		float x1 = position->get(i);
        float v1 = velocity->get(i);
        float a1 = acceleration->get(i);
        //k2 component
        float x2 = position->get(i) + .5f*v1*dt;
        float v2 = velocity->get(i) + .5f*a1*dt;
        float a2 = acceleration->get(i);
        //k3 component
        float x3 = position->get(i) + .5f*v2*dt;
        float v3 = velocity->get(i) + .5f*a2*dt;
        float a3 = acceleration->get(i);
        //k4 component
        float x4 = position->get(i) + v3*dt;
        float v4 = velocity->get(i) + a3*dt;
        float a4 = acceleration->get(i);
        //weighted average gives new position and velocity
        position->set(i, position->get(i) + (dt/6.f)*(v1 + 2.f*v2 + 2.f*v3 + v4));
        velocity->set(i, velocity->get(i) +(dt/6.f)*(a1 + 2.f*a2 + 2.f*a3 + a4));
	}
}

